#include "helicopter.h"
#include <cmath>
#include "helix.h"
#include "rotor.h"
#include "thread.h"
#include "crate.h"

const float Helicopter::zRotationAmount = PI/32;
const float Helicopter::threadExtensionAmount = 0.05f;
const Point Helicopter::accelerateUpAmount = Point(0,1,0);
const Point Helicopter::accelerateDownAmount = Point(0,-1,0);
const Point Helicopter::accelerateLeftAmount = Point(-1,0,0);
const Point Helicopter::accelerateRightAmount = Point(1,0,0);
const Point Helicopter::ascendAmount= Point(0,0,1);
const Point Helicopter::descendAmount= Point(0,0,-1);

const Point Helicopter::caccel = Point(0.05f,0.05f,0.05f);
const float Helicopter::maxSpeed = 2.0f;
const float Helicopter::airDrag = 0.9f;
const float Helicopter::inclinationFactor = 1.0f;

Helicopter::Helicopter(const Cube& cube, const Point& direction,const std::map<std::string,ObjectModelPtr>& models)
	: SceneObject(cube, direction,models.find(Helicopter::name())->second){
	this->zrotatedAngle = 0;

	Helix* helix = new Helix(models.find(Helix::name())->second);
	helix->setParent(this);

	Helix* rotor = new Rotor(models.find(Rotor::name())->second);
	rotor->setParent(this);
	Cube threadCube=Cube(Point(0.003,0.003,0.04),Point(-0.003,-0.003,-4.5 ) );
	Point center=this->dimensions.center();
	Cube clampCube=Cube(Point(center.x1-0.8,center.x2-0.8,center.x3-5.0),Point(center.x1+0.8,center.x2+0.8,center.x3-4.0));
	Thread* thread = new Thread(threadCube, clampCube, this->getDirection(),models);
	thread->setParent(this);

	this->addObject(Helix::name(),helix);
	this->addObject(Rotor::name(),rotor);
	this->addObject(Thread::name(),thread);
}

Helicopter::~Helicopter(){}

void Helicopter::timeStep(){
	calculate();
	this->dimensions.advance(this->speed);
	(this->findObject(Thread::name() ).findObject(Clamp::name()) ).move(this->speed.x1,this->speed.x2,this->speed.x3);
	for(std::map<std::string,SceneObject*>::iterator it=this->listObject.begin();it!=this->listObject.end();++it){
		it->second->timeStep();
	}
}

void Helicopter::calculate(){
	float dragAmount = this->speed.mod()*airDrag;
	if(dragAmount>=this->speed.mod())
		this->speed=Point(0,0,0);
	else
		this->speed=this->speed*(airDrag);

	Point oldInclinationAngle = inclinationAngle;
	for(unsigned int c=1;c<=3;c++){
		//this->inclinationAngle[c] = this->speed[c]*inclinationFactor;

		if(c==2)
			this->inclinationAngle[2] = (this->speed*this->direction)*inclinationFactor;
		else if(c==1){
			Point ortDir;
			ortDir = this->direction.cross(Point(0,0,1));
			ortDir = ortDir*(1/ortDir.mod());

			this->inclinationAngle[1] = (this->speed*ortDir)*inclinationFactor;
		}

		if(fabs(this->inclinationAngle[c])>PI/4)
			this->inclinationAngle[c] = PI/4*(fabs(this->inclinationAngle[c])/this->inclinationAngle[c]);

	}

	this->inclinationDiff = inclinationAngle-oldInclinationAngle;
}

void Helicopter::accelerate(const Point& dir){
	Matrix4x4 rot(this->zrotatedAngle,Point(0,0,1));
	this->speed += rot*Point(caccel.x1*dir.x1,caccel.x2*dir.x2,caccel.x3*dir.x3);
	if(this->speed.mod()>maxSpeed)
		this->speed = this->speed*(maxSpeed/this->speed.mod());
}


void Helicopter::setModelMatrix(){
	Point tr = this->dimensions.center();
	glTranslatef(tr.x1,tr.x2,tr.x3);
	glScalef(this->dimensions.width()/2,this->dimensions.depth()/2,this->dimensions.height()/2);


	glRotatef(this->zrotatedAngle*180/PI,0.0f,0.0f,1.0f);

	glRotatef(-this->inclinationAngle.x2*180/PI,1,0,0);
	glRotatef(this->inclinationAngle.x1*180/PI,0,1,0);

	glRotatef(180,0,0,1);
	glTranslatef(0,0,-0.3f);
}

void Helicopter::setSubModelMatrix(const std::string& name){
	if (name==Clamp::name() || name==Crate::name()){
		Point offset;
		if (name==Clamp::name() )
			offset=this->dimensions.center()-this->getClamp()->getDimensions().center();
		else
			offset=this->dimensions.center()-this->getClamp()->findObject(Grabable::name()).getDimensions().center();

		glTranslatef(offset.x1,offset.x2,offset.x3);

		glRotatef(this->zrotatedAngle*180/PI,0.0f,0.0f,1.0f);
		glRotatef(-this->inclinationAngle.x2*180/PI,1,0,0);
		glRotatef(this->inclinationAngle.x1*180/PI,0,1,0);
		glRotatef(-this->zrotatedAngle*180/PI,0.0f,0.0f,1.0f);

		glTranslatef(-offset.x1,-offset.x2,-offset.x3);
	}else if(name==Thread::name()){
		Point tr = this->dimensions.center();
		glTranslatef(tr.x1,tr.x2,tr.x3);
		glScalef(this->dimensions.width(),this->dimensions.depth(),this->dimensions.height());

		glRotatef(this->zrotatedAngle*180/PI,0.0f,0.0f,1.0f);

		glRotatef(-this->inclinationAngle.x2*180/PI,1,0,0);
		glRotatef(this->inclinationAngle.x1*180/PI,0,1,0);
		glScalef(1.0f,1.0f,1/this->dimensions.height());
	}else{
		Point tr = this->dimensions.center();
		glTranslatef(tr.x1,tr.x2,tr.x3);
		glScalef(this->dimensions.width(),this->dimensions.depth(),this->dimensions.height());

		glRotatef(this->zrotatedAngle*180/PI,0.0f,0.0f,1.0f);

		glRotatef(-this->inclinationAngle.x2*180/PI,1,0,0);
		glRotatef(this->inclinationAngle.x1*180/PI,0,1,0);
	}
}

void Helicopter::move(float dx,float dy,float dz){
	SceneObject::move(dx,dy,dz);
	SceneObject& tr= this->findObject(Thread::name());
	tr.move(dx,dy,dz);
}

void Helicopter::rotate(float angle){
	this->zrotatedAngle += angle;
	this->dimensions.rotate(angle,this->dimensions.center(),Point(0,0,1));
	Matrix4x4 m(angle,Point(0,0,1));
	this->direction=m* this->getDirection();

	SceneObject& tr= this->findObject(Thread::name());
	tr.setDirection(this->getDirection());
	tr.rotate(angle);
}

Clamp* Helicopter::getClamp(){
	return (& dynamic_cast<Clamp&>(this->findObject(Thread::name()).findObject(Clamp::name())) );

}

void Helicopter::extendThread(float offset){
	dynamic_cast<Thread&>(this->findObject(Thread::name())).extend(offset);
}

void Helicopter::shrinkThread(float offset){
	dynamic_cast<Thread&>(this->findObject(Thread::name())).shrink(offset);
}
